/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
*
* FILENAME:
* camera.h
*
* PURPOSE:
*  Provide video positioning system
*
* CREATED:
* 05/2008 by Cory Dixon
*
***********************************************************************/

#ifndef _CAMERA_H
#define _CAMERA_H

#include "vps.h"
#include <gtk/gtk.h>


// library configuration
#define USE_FFMPEG_SWS
#undef  USE_FFMPEG_CONVERT

#undef  CAM_RUN_DEINTERLACE  // define this if you want to have option of deinterlacing
#undef  USE_AVPICTURE_DEINTERLACE  // define this if you want to use libavpicture

// includes
#include <linux/videodev.h>
#include <linux/videodev2.h>
#include <cvaux.h>
#include <highgui.h>

extern "C" {
#ifdef USE_FFMPEG_SWS
	#include <libswscale/swscale.h>
#endif
	#include <libavcodec/avcodec.h>
}

#include "timeLib.h"
#include "matrix.h"
#include "String.h"

// helper macro
#define CLEAR(x) memset(&(x), 0, sizeof (x))

#ifdef CAM_RUN_DEINTERLACE
 #define PIXFMT_V4L2    V4L2_PIX_FMT_YUV420 // is VIDEO_PALETTE_YUV420P
 #define PIXFMT_AV      PIX_FMT_YUV420P
#else
 #define PIXFMT_V4L2    V4L2_PIX_FMT_BGR24 // is actually VIDEO_PALETTE_BGR24
 #define PIXFMT_AV      PIX_FMT_BGR24
#endif

// Camera class object
class Camera
{
	typedef enum {
		INPUT_METHOD_READ,
		INPUT_METHOD_MMAP,
		INPUT_METHOD_USERPTR,
	} Input_Method;

	struct buffer {
		void * start;
		size_t length;
	};

	protected:
		unsigned int id;   // camera ID

		float fps_d;    // fps is not supported by bttv
		int frameCount;
		float fps;         // measured fps
		TimeStamp ts_last; // time of last image
		TimeStamp ts_fps;  // time of last fps calculation

		// camera window
		unsigned int width;
		unsigned int height;

		unsigned int maxwidth;
		unsigned int maxheight;
		unsigned int minwidth;
		unsigned int minheight;

		// camera device
		int fd;
		String devName;
		unsigned int channel;
		Input_Method inputMethod;

		// V4L2 structs for video device
		struct v4l2_capability capability;
		struct v4l2_streamparm streamparm;
		v4l2_std_id video_std;
		struct v4l2_cropcap cropcap;
		struct v4l2_crop crop;
		struct v4l2_format fmt;
		v4l2_buf_type buf_type;

		// V4L structs
		struct video_capability vidcap;

		// image buffers
		struct buffer *buffers;
		unsigned int n_buffers;
		unsigned int buffer_size;

		// memory indices
		int frame_ind; // frame to trigger capture
		int img_ind;   // frame of most recent read

		// for image conversion
#ifdef USE_FFMPEG_SWS
		struct SwsContext *img_convert_ctx;
#endif

		// image fonts
		CvFont font;
		char videoText[255];

		// private member functions for initialization
		void init_read();
		void init_mmap();
		void init_userp();
		int set_defaultCrop();
		void free_buffers();
		void fill_bgr_img();

		void deinterlace();	
		// These images both point to the same shared memory area, therefore
		// if the camera deinterlaces the image, img_frame will also be the
		// same deinterlaced image.
		AVFrame *img_deint; // most recent deinterlaced image
		AVFrame *img_frame; // most recent image in buffer with YUV format
		bool deinterlaceVideo;

		String timeStr(char*format= "%a-%b-%d-%Y_%H-%M-%S" );

		// undistort bgr image
		bool undistortVideo;

		//uint8_t *deint_buffer;

		// image buffer for opencv
		IplImage *img_bgr;    // most recent image in BGR format
		IplImage *img_rgb;    // most recent image in BGR format
		GdkPixbuf *gdkImg;

		CvMat    *img_bg_mat; // actual background accumlator
		IplImage *img_bg_acc;  // floating point image of background accumulator

		IplImage *img_bg;  // uint8 image of background accumulator
		IplImage *img_fg;  // image of foreground
		IplImage *img_gray;  // image of foreground
		IplImage *fg_binary; // bianry foreground
		bool getBackgroundImg;
		bool useBackgroundSub;

		// video video writer
		CvVideoWriter *video_writer;

		//-----------------------------------------------------
		// Camera Calibration
		//-----------------------------------------------------
		// intrinsic_matrix 3x3
		//    The camera matrix (A) [fx 0 cx; 0 fy cy; 0 0 1]. 
		CvMat *intrinsic;
		// distortion_coeffs 4x1 or 1x4
		//    The vector of distortion coefficients, [k1, k2, p1, p2]. 
		CvMat *distortion;
		// translation vector
		//    translation of to world coordinates from camera optical axis, in camera axis 
		CvMat *translation;
		// rotation vector 3x1 or 1x3
		//    Rodrigues rotation vector (compact representation of a rotation matrix) 
		//    to world frame from camera frame
		CvMat *rotation;

	public:
		// if width or height are not specified, the camera will initialize
		// to the largest possible frame
		Camera( unsigned int myId, const char* dev=NULL, unsigned width=0, unsigned height=0 );
		virtual ~Camera( );

		virtual void cap_dump();
		bool useCam;
		bool saveVideo;

		virtual int open(const char* dev = NULL);
		virtual int close();
		virtual int init();
		virtual int set_size(int w, int h); // (0,0) will set max size, (-1,-1) will set min size

		virtual void setIntrinsic( float intrin[] );
		virtual void setDistortion( float dist[] );
		virtual void setRotation( float rot[] );    // given as a Rodrigues vector
		virtual void setTranslation( float t[] );   // translation of camera in world coordinates

		virtual void setCam_undistortFrame(bool u=0) { undistortVideo = u; };

		virtual void undistort_points(Matrix imgPoints, Matrix &opticalPoints);
		virtual void getWorldPoints(Matrix imgPoints, Matrix &worldPoints);
		virtual void getImgPoints(Matrix worldPoints, Matrix &imgPoints);

		virtual void devDelete();

		int get_fd() { return fd; };
		int get_frameCount() { return frameCount; };
		int get_imgInd() { return img_ind; };
		char const * get_dev() { return devName.c_str(); }

		virtual int start_capture();
		virtual int stop_capture();
		virtual int set_fps(float f);
		float get_fps() { return fps; };
		double lastImg_time() { return ts_last.secOfDay(); };//return ts_last.time(); };

		virtual int trigger_capture();
		virtual int read_frame();
		//void process_image(const void * p);
		virtual void process_image();
		void setCam_deinterlaceFrame(bool d = true) { deinterlaceVideo = d; };	

		int init_videoFile(const char *filePath = NULL, const char *fileName = NULL);
		void close_videoFile();
		int write_videoFrame();
		int saveSnapshot(const char *filePath = NULL, const char* fileName = NULL);

		void setCaptureBackground(bool val=true);
		void setUseBackground(bool val=true);
		int saveBackground(String file);
		int loadBackground(String file);

		IplImage * get_img(); // get most current image
		GdkPixbuf * get_pixbuf(); // get most current image
		IplImage *const get_bgr_img(); // get most current bgr image
		IplImage *const get_bg_img();  // get background image
		IplImage *const get_fg_img();  // get only foreground components
		IplImage *const get_fg_bin();  // get binary foreground image after threshold
		
		// undistort bgr image
		void undistortImg(IplImage *t=NULL);

		// public member variables so its easy to access for gtk signals
		float bin_thresh;       // threshold to make binary foreground image
		float led_thresh;       // threshold to make binary image for marker image
		int   dilate;           // number of time to dilate image
};
#endif
